#include "fleettextcommandtransfer.h"
#include "fleetservice.h"
#include <QDebug>
#include <QTimer>
#include <QDateTime>

FleetTextCommandTransfer::FleetTextCommandTransfer(FleetService *fleetServiceIn,int maxLogLine)
{
    fleetService=fleetServiceIn;
    maxFleetCatchLineForDebug=maxLogLine;
}

int FleetTextCommandTransfer::setFleetTextCommandMaxLogLine(int maxIn)
{
    maxFleetCatchLineForDebug=maxIn;
    return 1;
}

int FleetTextCommandTransfer::parserTexCommand(QString txtCommand)
{
    if(0==txtCommand.size())
    {
        return 0;
    }

    //分析字符串
    QString tmp_frame=txtCommand;
    //截取有效字符
    //开头处理
    QString startFlag = "\n";
    int startIndex=tmp_frame.indexOf(startFlag);
    if(0>startIndex)
    {
        qDebug()<<"FleetTextCommandTransfer::processClientRequest command format error:"<<tmp_frame;
        return -1;
    }
    tmp_frame=tmp_frame.mid(startIndex+1);
    //末尾处理
    startFlag = '\r';
    startIndex=tmp_frame.indexOf(startFlag);
    if(0>startIndex)
    {
        return -2;
    }
    tmp_frame=tmp_frame.mid(0,startIndex);

    //提取header
    QString header;
    startFlag = '#';
    startIndex=tmp_frame.indexOf(startFlag);
    if(0>startIndex)
    {
        header=tmp_frame;
    }
    else
    {
//        header=tmp_frame.mid(0,startIndex);
        header=tmp_frame.section("#", 0, 0);
    }
    if("PATHAUTHORITY"!=header)
    {
        saveFleetCommand(tmp_frame);
    }

    //根据命令类别作不同处理
    int returnKey;
    if("DE"==header)
    {
        returnKey=processClearPath();
    }
    else if("SHUTDOWN"==header)
    {
        returnKey=processShutDown(tmp_frame);
    }
    else if("TG"==header)
    {
        returnKey=processAddSimplePath(tmp_frame);
    }
    else if("POINT"==header)
    {
        returnKey=processAddComplexPath(tmp_frame);
    }
    else if("HT"==header)
    {
        returnKey=processSetForkHeight(tmp_frame);
    }
    else if("PA"==header)
    {
        returnKey=processPause();
    }
    else if("ST"==header)
    {
        returnKey=processStop();
    }
    else if("GO"==header)
    {
        returnKey=processStart();
    }
    else if("MUSIC"==header)
    {
        returnKey=processMusic(tmp_frame);
    }
    else if("MAP"==header)
    {
        returnKey=processLoadMap(tmp_frame);
    }
    else if("LIFT"==header)
    {
        returnKey=processLift(tmp_frame);
    }
    else if("ROLLER"==header)
    {
        returnKey=processRoller(tmp_frame);
    }
    else if("WAIT"==header)//按照队列执行等待
    {
        returnKey=processAddWait(tmp_frame);
    }
    else if("SETVEL"==header)
    {
        returnKey=processSetGlobalVelocity(tmp_frame);
    }
    else if("CLEARMSG"==header)
    {
        returnKey=processClearMessage();
    }
    else if("CHARGE"==header)
    {
        returnKey=processCharge(tmp_frame);
    }
    else if("MOVEARM"==header)
    {
        returnKey=processMoveRobotArm(tmp_frame);
    }
    else if("AUTODRIVE"==header)
    {
        returnKey=processAutoDrive(tmp_frame);
    }
    else if("WAITDI"==header)
    {
        returnKey=processWaitDi(tmp_frame);
    }
    else if("SETDO"==header)
    {
        returnKey=processSetDo(tmp_frame);
    }
    else if("STARTINTERNALPROGRAM"==header)
    {
        returnKey=processSTARTINTERNALPROGRAM(tmp_frame);
    }
    else if("CEASEINTERNALPROGRAM"==header)
    {
        returnKey=processCEASEINTERNALPROGRAM(tmp_frame);
    }
    else if("STOPINTERNALPROGRAM"==header)
    {
        returnKey=processSTOPINTERNALPROGRAM(tmp_frame);
    }
    else if("LOADINTERNALPROGRAM"==header)
    {
        returnKey=processLOADINTERNALPROGRAM(tmp_frame);
    }
    else if("SETTRIGOBJLENGTH"==header)
    {
        returnKey=processSETTRIGOBJLENGTH(tmp_frame);
    }
    else if("CLEARERROR_RESTOR_RUN"==header)
    {
        returnKey=processClearErrorRestoreRun(tmp_frame);
    }
    else if("SET_REGISTER_VALUE"==header)
    {
        returnKey=processSetRegisterValue(tmp_frame);
    }
    else if("SETBARRIER"==header)
    {
        returnKey=processSetBarrier(tmp_frame);
    }
    else if("SETBORDERID"==header)
    {
        returnKey=processSetBorderId(tmp_frame);
    }
    else if("SETVBORDEROFFSET"==header)
    {
        returnKey=processSetVehicleBorderOffset(tmp_frame);
    }
    else if("PATHAUTHORITY"==header)
    {
        returnKey=processPathAuthority(tmp_frame);
    }
    else if("SCANCAMERATRIGSTART"==header)
    {
        returnKey=processScanCAMERATRIGSTART(tmp_frame);
    }
    else if("SCANCAMERATRIGEND"==header)
    {
        returnKey=processScanCAMERATRIGEND(tmp_frame);
    }
    else if("MOVEMDI"==header)
    {
        returnKey=processMoveMonitorDi(tmp_frame);
    }
    else
    {
        qDebug()<<"txtCommand,unknown header"<<header;
        return -1;
    }


    return returnKey;
}

int FleetTextCommandTransfer::getFleetCommandLog(std::queue<string> &fleetCommandLogOut)
{
    usingMutex.lock();
    fleetCommandLogOut=fleetCommandLog;
    usingMutex.unlock();
    return 1;
}


int FleetTextCommandTransfer::processClearPath()
{
    qDebug()<<"FleetTextCommandTransfer::processClearPath";
    return fleetService->clearCommand(userInfo);

}

int FleetTextCommandTransfer::processPause()
{
    qDebug()<<"FleetTextCommandTransfer::processPause";
    return fleetService->pause(userInfo);

}

int FleetTextCommandTransfer::processStart()
{
    qDebug()<<"FleetTextCommandTransfer::processStart";
    return fleetService->run(userInfo);
}

int FleetTextCommandTransfer::processStop()
{
    qDebug()<<"FleetTextCommandTransfer::processStop";
    return fleetService->stop(userInfo);
}

int FleetTextCommandTransfer::processSetForkHeight(QString commandStr)
{
    return 0;
}

int FleetTextCommandTransfer::processAddSimplePath(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processAddSimplePath";
    LaserPathParameter paraIn;
    QString tmpStrVal;
    //提取direction
    tmpStrVal=commandStr.section("#",1,1);
    if("S"==tmpStrVal)
    {
        paraIn.isPositiveDirection=true;
    }
    else if("N"==tmpStrVal)
    {
        paraIn.isPositiveDirection=false;
    }
    else
    {
        return -1;
    }
    //提取location
    tmpStrVal=commandStr.section("#",2,2);
    if("T"==tmpStrVal)
    {
        paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_MCCRUM;
    }
    else if("M"==tmpStrVal)
    {
        paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
    }
    else
    {
        return -1;
    }
    //提取attribute
    tmpStrVal=commandStr.section(QRegExp("[#:]"), 3, 3);
    if("Q"==tmpStrVal)//Ｑ经过停止点
    {
        paraIn.CNT_Value=0;
    }
    else if("A"==tmpStrVal)//H 经过点半米弯
    {
        paraIn.CNT_Value=100;
    }
    else
    {
        return -1;
    }

    //提取X
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 1, 1);
    paraIn.targetPosition.x=tmpStrVal.toDouble();
    //提取Y
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 2, 2);
    paraIn.targetPosition.y=tmpStrVal.toDouble();
    //提取R
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 3, 3);
    paraIn.targetPosition.rotate=tmpStrVal.toDouble()*M_PI/180.0;

    //默认
    paraIn.moveFreeMode=E_MOVE_FREE_IMU;                 // 自由导航模式
    paraIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
    paraIn.pointSensorType=EM_MOVE_FREE_DP;      // 自由导航指令点位模式
    paraIn.velocityRatio=100;                          // 运行速度比
    paraIn.pointsValue=0;                            // DP单点数值
    paraIn.registerPointsValue=0;                    // DPR寄存器点数值
    paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;   // 自由导航指令运行轨迹类型；  AUTO:自由轨迹  LINE:直线轨迹  SPLINE:样条轨迹
    paraIn.allowAccuracy=0.25;//停车精度，单位米
    paraIn.pointIndex=0;//点位类型索引,vmark类型
    paraIn.laserTypeNum=0;
    paraIn.isDetectBarrier=true; //是否探测障碍物
    paraIn.barrierRunMode=0;

    return fleetService->addLaserPath(userInfo,paraIn);
}

int FleetTextCommandTransfer::processAddWait(QString commandStr)
{

    return fleetService->addWait(userInfo);
}

int FleetTextCommandTransfer::processAddComplexPath(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processAddComplexPath";
    LaserPathParameter paraIn;
    QString tmpStrVal;
    //提取direction
    tmpStrVal=commandStr.section("#",1,1);
    if("S"==tmpStrVal)
    {
        paraIn.isPositiveDirection=true;
    }
    else if("N"==tmpStrVal)
    {
        paraIn.isPositiveDirection=false;
    }
    else
    {
        return -1;
    }
    //提取location
    tmpStrVal=commandStr.section("#",2,2);
    if("T"==tmpStrVal)
    {
        paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_MCCRUM;
    }
    else if("M"==tmpStrVal)
    {
        paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
    }
    else
    {
        return -1;
    }
    //提取attribute
    tmpStrVal=commandStr.section(QRegExp("[#:]"), 3, 3);
    if("Q"==tmpStrVal)//Ｑ经过停止点
    {
        paraIn.CNT_Value=0;
    }
    else if("A"==tmpStrVal)//A 普通经过点
    {
        paraIn.CNT_Value=100;
    }
    else
    {
        return -1;
    }

    //提取X
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 1, 1);
    paraIn.targetPosition.x=tmpStrVal.toDouble();
    //提取Y
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 2, 2);
    paraIn.targetPosition.y=tmpStrVal.toDouble();
    //提取R
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 3, 3);
    paraIn.targetPosition.rotate=tmpStrVal.toDouble()*M_PI/180.0;

    //提取位置反馈类型
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 4, 4);
    if("AM"==tmpStrVal)
    {
        paraIn.pointSensorType=EM_MOVE_FREE_DP;      // 自由导航指令点位模式
    }
    else if("VMARK"==tmpStrVal)
    {
        paraIn.pointSensorType=EM_MOVE_FREE_VMARK;      // 自由导航指令点位模式
    }
    else if("REFLECT"==tmpStrVal)
    {
        paraIn.pointSensorType=EM_MOVE_FREE_REFLECT;      // 自由导航指令点位模式
    }
    else if("QR"==tmpStrVal)
    {
        paraIn.pointSensorType=EM_MOVE_FREE_QR;      // 自由导航指令点位模式
    }
    else
    {
        return -1;
    }

    //提取位置反馈类型index
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 5, 5);
    paraIn.pointIndex=tmpStrVal.toInt();//点位类型索引,vmark类型

    //提取激光滤波index
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 6, 6);
    paraIn.laserTypeNum=tmpStrVal.toInt();
    //提取vel
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 7, 7);
    paraIn.velocityRatio=tmpStrVal.toDouble();
    //提取detect
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 8, 8);
    if("DE0"==tmpStrVal)
    {
            paraIn.isDetectBarrier=true; //是否探测障碍物
            paraIn.barrierRunMode=E_BARRIER_RUN_MODE_COMMON;
    }
    else if("DE1"==tmpStrVal)
    {
            paraIn.isDetectBarrier=true; //是否探测障碍物
            paraIn.barrierRunMode=E_BARRIER_RUN_MODE_VMARK;
    }
    else if("DE2"==tmpStrVal)
    {
            paraIn.isDetectBarrier=true; //是否探测障碍物
            paraIn.barrierRunMode=E_BARRIER_RUN_MODE_NARROW_A;
    }
    else if("DE3"==tmpStrVal)
    {
            paraIn.isDetectBarrier=true; //是否探测障碍物
            paraIn.barrierRunMode=E_BARRIER_RUN_MODE_NARROW_B;
    }
    else if("NODE"==tmpStrVal)
    {
            paraIn.isDetectBarrier=false; //是否探测障碍物
    }
    else if("DONOTHING"==tmpStrVal)
    {
            paraIn.isDetectBarrier=true; //是否探测障碍物
            paraIn.barrierRunMode=E_BARRIER_RUN_MODE_DONOTHING;
    }
    else
    {
        qDebug()<<"error,unknow barrierRunMode";
        return -1;
    }
    //提取accuracy
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 9, 9);
    paraIn.allowAccuracy=tmpStrVal.toDouble();//停车精度，单位米

    //提取轨迹类型
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 10, 10);
    if("LINE"==tmpStrVal)
    {
            paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;   // 自由导航指令运行轨迹类型；  AUTO:自由轨迹  LINE:直线轨迹  SPLINE:样条轨迹
    }
    else if("ROTATE"==tmpStrVal)
    {
            paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_ROTATE;
    }
    else if("SPLINE"==tmpStrVal)
    {
            paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_SPLINE;
    }
    else
    {
        return -1;
    }
    //提取stationId
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 11, 11);
    paraIn.stationId=tmpStrVal.toInt();

    //提取pathId
    tmpStrVal=commandStr.section(QRegExp("[:,]"), 12, 12);
    paraIn.pathId=tmpStrVal.toInt();

    tmpStrVal=commandStr.section(QRegExp("[:,]"), 13, 13);
    if(tmpStrVal.size()>0)
    {
       paraIn.p1x=tmpStrVal.toDouble();
    }
    else
    {
        return -1;
    }

    tmpStrVal=commandStr.section(QRegExp("[:,]"), 14, 14);
    if(tmpStrVal.size()>0)
    {
       paraIn.p1y=tmpStrVal.toDouble();
    }
    else
    {
        return -1;
    }


    tmpStrVal=commandStr.section(QRegExp("[:,]"), 15, 15);
    if(tmpStrVal.size()>0)
    {
       paraIn.p2x=tmpStrVal.toDouble();
    }
    else
    {
        return -1;
    }


    tmpStrVal=commandStr.section(QRegExp("[:,]"), 16, 16);
    if(tmpStrVal.size()>0)
    {
       paraIn.p2y=tmpStrVal.toDouble();
    }
    else
    {
        return -1;
    }

//    //提取isRotatePrepare
//    tmpStrVal=commandStr.section(QRegExp("[:,]"), 13, 13);
//    if(tmpStrVal.size()>0)
//    {
//       paraIn.isRotatePrepare=tmpStrVal.toInt();
//    }
//    else
//    {
//        paraIn.isRotatePrepare=false;
//    }


//    //提取prepareAngle
//    tmpStrVal=commandStr.section(QRegExp("[:,]"), 14, 14);
//    if(tmpStrVal.size()>0)
//    {
//       paraIn.prepareAngle=tmpStrVal.toDouble();
//    }
//    else
//    {
//        paraIn.prepareAngle=0.0;
//    }

//    //提取controlPointRatio1
//    tmpStrVal=commandStr.section(QRegExp("[:,]"), 15, 15);
//    if(tmpStrVal.size()>0)
//    {
//       paraIn.controlPointRatio1=tmpStrVal.toDouble();
//    }
//    else
//    {
//        paraIn.controlPointRatio1=0.3;
//    }


//    //提取controlPointRatio2
//    tmpStrVal=commandStr.section(QRegExp("[:,]"), 16, 16);
//    if(tmpStrVal.size()>0)
//    {
//       paraIn.controlPointRatio2=tmpStrVal.toDouble();
//    }
//    else
//    {
//        paraIn.controlPointRatio2=0.3;
//    }

    //默认
    paraIn.moveFreeMode=E_MOVE_FREE_IMU;                 // 自由导航模式
    paraIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
    paraIn.pointsValue=0;                            // DP单点数值
    paraIn.registerPointsValue=0;                    // DPR寄存器点数值


    return fleetService->addLaserPath(userInfo,paraIn);
}

int FleetTextCommandTransfer::processMusic(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processMusic";
    MusicParameter paraIn;
    QString tmpStrVal;

    tmpStrVal=commandStr.section("#",1,1);
    paraIn.musicFileName=tmpStrVal;
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.musicVolume=tmpStrVal.toInt();
    tmpStrVal=commandStr.section("#",3,3);
    paraIn.musicPlayTimes=tmpStrVal.toInt();


    return fleetService->addMusic(userInfo,paraIn);
}

int FleetTextCommandTransfer::processRoller(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processRoller";
    RollerParameter paraIn;
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);

    if("LIN"==tmpStrVal)
    {
        paraIn.rollerDirectionType=E_ROLLER_LEFT_LOAD;
    }
    else if("LOUT"==tmpStrVal)
    {
        paraIn.rollerDirectionType=E_ROLLER_LEFT_DOWNLOAD;
    }
    else if("RIN"==tmpStrVal)
    {
        paraIn.rollerDirectionType=E_ROLLER_RIGHT_LOAD;
    }
    else if("ROUT"==tmpStrVal)
    {
        paraIn.rollerDirectionType=E_ROLLER_RIGHT_DOWNLOAD;
    }
    else
    {
        return -1;
    }
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.timeOutValue=tmpStrVal.toInt();

    return fleetService->addRoller(userInfo,paraIn);
}

int FleetTextCommandTransfer::processLift(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processLift";
    LiftParameter paraIn;
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);

    if("U"==tmpStrVal)
    {
        paraIn.liftType=E_LIFT_RISE;
    }
    else if("D"==tmpStrVal)
    {
        paraIn.liftType=E_LIFT_DOWN;
    }
    else if("UM"==tmpStrVal)
    {
        paraIn.liftType=E_LIFT_RISE_MIDDLE;
    }
    else if("DM"==tmpStrVal)
    {
        paraIn.liftType=E_LIFT_DOWN_MIDDLE;
    }
    else
    {
        return -1;
    }
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.timeOutValue=tmpStrVal.toInt();

    tmpStrVal=commandStr.section("#",3,3);
    paraIn.isMonitorCargo=tmpStrVal.toInt();

    return fleetService->addLifter(userInfo,paraIn);
}

int FleetTextCommandTransfer::processLoadMap(QString commandStr)
{
     qDebug()<<"FleetTextCommandTransfer::processLoadMap";
    LoadMapParameter paraIn;
    QString tmpStrVal;

    tmpStrVal=commandStr.section("#",1,1);
    paraIn.mapName=tmpStrVal;
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.x=tmpStrVal.toDouble();
    tmpStrVal=commandStr.section("#",3,3);
    paraIn.y=tmpStrVal.toDouble();
    tmpStrVal=commandStr.section("#",4,4);
    paraIn.r=tmpStrVal.toDouble();


    return fleetService->addLoadMap(userInfo,paraIn);
}

int FleetTextCommandTransfer::processSetGlobalVelocity(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSetGlobalVelocity";
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);
    double vel=tmpStrVal.toDouble();

    return fleetService->addSetGlobalVelocity(userInfo,vel);
}


int FleetTextCommandTransfer::processCharge(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processCharge";
    ChargeParameter paraIn;
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);
    if("V"==tmpStrVal)
    {
        paraIn.chargingFinishedType=E_CHARGING_CAPARITY;
    }
    else if("T"==tmpStrVal)
    {
        paraIn.chargingFinishedType=E_CHARGING_TIME;
    }
    else if("N"==tmpStrVal)
    {
        paraIn.chargingFinishedType=E_CHARGING_FIRST_FINISHED;
    }
    else if("A"==tmpStrVal)
    {
        paraIn.chargingFinishedType=E_CHARGING_ON;
    }
    else if("B"==tmpStrVal)
    {
        paraIn.chargingFinishedType=E_CHARGING_OFF;
    }
    else
    {
        return -1;
    }

    tmpStrVal=commandStr.section("#",2,2);
    paraIn.chargingTime=tmpStrVal.toDouble();
    tmpStrVal=commandStr.section("#",3,3);
    paraIn.chargingCaparity=tmpStrVal.toInt();

    return fleetService->addCharge(userInfo,paraIn);
}

int FleetTextCommandTransfer::processClearMessage()
{
    qDebug()<<"FleetTextCommandTransfer::processClearMessage";
    return fleetService->addClearMessage(userInfo);
}

int FleetTextCommandTransfer::processMoveRobotArm(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processMoveRobotArm";
    RobotArmMoveParameter paraIn;
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);
    if("J"==tmpStrVal)
    {
        paraIn.moveType=0;
    }
    else if("L"==tmpStrVal)
    {
        paraIn.moveType=1;
    }
    else if("C"==tmpStrVal)
    {
        paraIn.moveType=2;
    }
    else
    {
        return -1;
    }

    tmpStrVal=commandStr.section("#",2,2);
    paraIn.velocityRatio=tmpStrVal.toDouble();
    tmpStrVal=commandStr.section("#",3,3);
    bool isJointPoint;
    if("JX"==tmpStrVal)
    {
        isJointPoint=true;
    }
    else if("DX"==tmpStrVal)
    {
        isJointPoint=false;
    }
    else
    {
        return -2;
    }

    tmpStrVal=commandStr.section("#",4,4);
    double tmpAxisCount=tmpStrVal.toInt();
    if(isJointPoint)
    {
        paraIn.targetPoint.jointFlag=0;
        paraIn.targetPoint.positionValue.resize(tmpAxisCount);
        for(int i=0;i<tmpAxisCount;i++)
        {
            tmpStrVal=commandStr.section("#",5+i,5+i);
            paraIn.targetPoint.positionValue[i]=tmpStrVal.toDouble();
        }
    }
    else
    {
        paraIn.targetPoint.jointFlag=1;
        paraIn.targetPoint.positionValue.resize(6);
        for(int i=0;i<6;i++)
        {
            tmpStrVal=commandStr.section("#",5+i,5+i);
            paraIn.targetPoint.positionValue[i]=tmpStrVal.toDouble();
        }
    }
    paraIn.CNT_Value=0;


    return fleetService->addRobotArmMove(userInfo,paraIn);
}

int FleetTextCommandTransfer::processAutoDrive(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processAutoDrive";
    AutoDriveParameter paraIn;
    QString tmpStrVal;

    tmpStrVal=commandStr.section("#",1,1);
    paraIn.targetStationId=tmpStrVal.toInt();
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.speedRatio=tmpStrVal.toInt();
    paraIn.speedRatio=paraIn.speedRatio;
//    tmpStrVal=commandStr.section("#",3,3);
//    paraIn.musicPlayTimes=tmpStrVal.toInt();


    return fleetService->addAutoDrive(userInfo,paraIn);
}

int FleetTextCommandTransfer::processWaitDi(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processWaitDi";
    WaitDiParameter paraIn;
    QString tmpStrVal;

    tmpStrVal=commandStr.section("#",1,1);
    paraIn.diIndex=tmpStrVal.toInt();
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.waitCondition=tmpStrVal.toInt();


    return fleetService->addWaitDi(userInfo,paraIn);
}

int FleetTextCommandTransfer::processSetDo(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSetDo";
    SetDoParameter paraIn;
    QString tmpStrVal;

    tmpStrVal=commandStr.section("#",1,1);
    paraIn.doIndex=tmpStrVal.toInt();
    tmpStrVal=commandStr.section("#",2,2);
    paraIn.doValue=tmpStrVal.toInt();


    return fleetService->addSetDo(userInfo,paraIn);
}

int FleetTextCommandTransfer::processShutDown(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processShutDown";

    return fleetService->shutDownAgvSystem(userInfo);
}

int FleetTextCommandTransfer::processSTARTINTERNALPROGRAM(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSTARTINTERNALPROGRAM";

    return fleetService->startInternalProgram(userInfo);


}

int FleetTextCommandTransfer::processCEASEINTERNALPROGRAM(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processCEASEINTERNALPROGRAM";

    return fleetService->ceaseInternalProgram(userInfo);
}

int FleetTextCommandTransfer::processSTOPINTERNALPROGRAM(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processCEASEINTERNALPROGRAM";

    return fleetService->stopInternalProgram(userInfo);
}

int FleetTextCommandTransfer::processLOADINTERNALPROGRAM(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processLOADINTERNALPROGRAM";
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);

    return fleetService->loadInternalProgram(userInfo,tmpStrVal.toStdString());
}

int FleetTextCommandTransfer::processSETTRIGOBJLENGTH(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSETTRIGOBJLENGTH";
    QString tmpStrVal1,tmpStrVal2;
    tmpStrVal1=commandStr.section("#",1,1);
    tmpStrVal2=commandStr.section("#",2,2);

    return fleetService->setVisionTrigObjectLength(userInfo,tmpStrVal1.toInt(),tmpStrVal2.toDouble());
}

int FleetTextCommandTransfer::processClearErrorRestoreRun(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processClearErrorRestoreRun";

    return fleetService->clearErrorAndRestoreRun(userInfo);
}

int FleetTextCommandTransfer::processSetRegisterValue(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSetRegisterValue";
    QString tmpStrVal1,tmpStrVal2,tmpStrVal3,tmpStrVal4;
    tmpStrVal1=commandStr.section("#",1,1);
    tmpStrVal2=commandStr.section("#",2,2);
    tmpStrVal3=commandStr.section("#",3,3);
    tmpStrVal4=commandStr.section("#",4,4);

    return fleetService->setRegisterValue(userInfo,tmpStrVal1.toInt(),tmpStrVal2.toInt(),
                                          tmpStrVal3.toInt(),tmpStrVal4.toDouble());
}

int FleetTextCommandTransfer::processSetBarrier(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSetBarrier";
    QString tmpStrVal;
    tmpStrVal=commandStr.section("#",1,1);
    E_BARRIER_RUN_MODE modeIn;
    if("DE0"==tmpStrVal)
    {
            modeIn=E_BARRIER_RUN_MODE_COMMON;
    }
    else if("DE1"==tmpStrVal)
    {
            modeIn=E_BARRIER_RUN_MODE_VMARK;
    }
    else if("DE2"==tmpStrVal)
    {
            modeIn=E_BARRIER_RUN_MODE_NARROW_A;
    }
    else if("DE3"==tmpStrVal)
    {
            modeIn=E_BARRIER_RUN_MODE_NARROW_B;
    }
    else if("NODE"==tmpStrVal)
    {
            modeIn=E_BARRIER_RUN_MODE_NONE;
    }
    else
    {
        qDebug()<<"error,unknow barrierRunMode";
        return -1;
    }

    return fleetService->setBarrierRunMode(userInfo,modeIn);
}

int FleetTextCommandTransfer::processSetBorderId(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSetBorderId";
    QString tmpStrVal1;
    tmpStrVal1=commandStr.section("#",1,1);

    return fleetService->setCurrentVehicleBorderId(userInfo,tmpStrVal1.toInt());
}

int FleetTextCommandTransfer::processSetVehicleBorderOffset(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processSetVehicleBorderOffset";
    QString tmpStrVal1,tmpStrVal2,tmpStrVal3,tmpStrVal4;
    tmpStrVal1=commandStr.section("#",1,1);
    tmpStrVal2=commandStr.section("#",2,2);
    tmpStrVal3=commandStr.section("#",3,3);
    tmpStrVal4=commandStr.section("#",4,4);

    return fleetService->setVehicleBorderOffset(userInfo,tmpStrVal1.toDouble(),tmpStrVal2.toDouble(),
                                                tmpStrVal3.toDouble(),tmpStrVal4.toDouble());
}

int FleetTextCommandTransfer::processPathAuthority(QString commandStr)
{

    int pathCount=commandStr.section("#",1,1).toInt();
    QVector<int> pathList;
    for(int i=0;i<pathCount;i++)
    {
        pathList.append(commandStr.section("#",2+i,2+i).toInt());
    }
    //授权站点
    int stationCount=commandStr.section("#",2+pathCount,2+pathCount).toInt();
    QVector<int> stationList;
    for(int i=0;i<stationCount;i++)
    {
        stationList.append(commandStr.section("#",3+pathCount+i,3+pathCount+i).toInt());
    }


    fleetService->setPathAuthority(pathList,stationList);
//    qDebug()<<"FleetTextCommandTransfer::processPathAuthority"<<pathList;
    return 1000;
}

int FleetTextCommandTransfer::processScanCAMERATRIGSTART(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processScanCAMERATRIGSTART";
    QString tmpStrVal1;
    tmpStrVal1=commandStr.section("#",1,1);
    QString tmpStrVal2;
    tmpStrVal2=commandStr.section("#",2,2);

    return fleetService->setMotionTrigPara(tmpStrVal1.toInt(),tmpStrVal2.toDouble());
}

int FleetTextCommandTransfer::processScanCAMERATRIGEND(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processScanCAMERATRIGEND";
    QString tmpStrVal1;
    tmpStrVal1=commandStr.section("#",1,1);

    return fleetService->stopMotionTrig(tmpStrVal1.toInt());
}

int FleetTextCommandTransfer::processMoveMonitorDi(QString commandStr)
{
    qDebug()<<"FleetTextCommandTransfer::processMoveMonitorDi";
    bool isOr;
    QVector<int> diIndex;
    QVector<int> diValue;

    QString tmpStrVal1;
    tmpStrVal1=commandStr.section("#",1,1);
    QString tmpStrVal2;
    tmpStrVal2=commandStr.section("#",2,2);
    if(tmpStrVal1=="AND")
    {
        isOr=false;
    }
    else if(tmpStrVal1=="OR")
    {
        isOr=true;
    }
    else
    {
        return false;
    }
    int tmpDiCount=tmpStrVal2.toInt();
    for(int i=0;i<tmpDiCount;i++)
    {
       diIndex.append(commandStr.section("#",3+i*2,3+i*2).toInt()-1);
       diValue.append(commandStr.section("#",4+i*2,4+i*2).toInt());
    }

    return fleetService->addMonitorDi_fleet(isOr,diIndex,diValue);
}

int FleetTextCommandTransfer::saveFleetCommand(QString frameIn)
{
    usingMutex.lock();
    QDateTime current_date_time =QDateTime::currentDateTime();
    QString tmpStr=current_date_time.toString("yyyy.MM.dd-hh:mm:ss");
    frameIn=tmpStr+": "+frameIn;

//    fleetCommandLog.append(frameIn);
    fleetCommandLog.push(frameIn.toStdString());
    if(fleetCommandLog.size()>maxFleetCatchLineForDebug)
    {
//        fleetCommandLog.dequeue();
        fleetCommandLog.pop();
    }
    usingMutex.unlock();
    return 1;

}


